#include "ros/ros.h"
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <rosbag/bag.h>
#include "robot_control/Trajectory.h"
#include "robot_control/Odometry.h"
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/PointCloud2.h>


typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image,
				sensor_msgs::Image,
				sensor_msgs::PointCloud2> MySyncPolicy;

rosbag::Bag bag;
long sampling = 0;
long rate = 10;

void cameraCallback (
		const sensor_msgs::ImageConstPtr& visual_img_msg, 
		const sensor_msgs::ImageConstPtr& depth_img_msg, 
		const sensor_msgs::PointCloud2ConstPtr& point_cloud) {
	if ((sampling % rate) == 0) {
		bag.write("/camera/rgb/image_color", ros::Time::now(), visual_img_msg);
		bag.write("/camera/depth/image", ros::Time::now(), depth_img_msg);
		bag.write("/camera/rgb/points", ros::Time::now(), point_cloud);
		ROS_INFO_STREAM("Wrote to bagfile " << bag.getFileName());
		ROS_INFO("saving bag");
	}
			sampling++;
}

void odometry_callback(const robot_control::TrajectoryConstPtr& odo_msg) {
//	if ((sampling % rate) == 0) {
	  bag.write("robot_trajectory", ros::Time::now(), odo_msg);
//	}
}

void encoder_callback(const robot_control::OdometryConstPtr& encoder_msg){
  bag.write("robot_odometry",ros::Time::now(),encoder_msg);
}

int main(int argc, char **argv) {
	if (argc != 2) {
    ROS_ERROR("File argument missing. Usage: bag_sampler <filename>");
		return -1;
	}
	ros::init(argc, argv, "bag_sampler");
  ros::NodeHandle nh;

	bag.open(argv[1], rosbag::bagmode::Write);
	message_filters::Subscriber<sensor_msgs::Image> visual_sub(
			nh, "/camera/rgb/image_color", 1000);
  message_filters::Subscriber<sensor_msgs::Image> depth_sub(
			nh,"/camera/depth/image", 1000);
	message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub(
			nh, "/camera/rgb/points", 1000);

	ros::Subscriber odometry_sub = nh.subscribe<robot_control::Trajectory>(
			"robot_trajectory", 1000, odometry_callback);

 // ros::Subscriber encoder_sub = nh.subscribe<robot_control::Odometry>(
  //    "robot_odometry", 1000, encoder_callback);

	message_filters::Synchronizer<MySyncPolicy> sync(
			MySyncPolicy(1000), visual_sub, depth_sub, cloud_sub);

	ROS_INFO("Ready to subsample kinect");
	sync.registerCallback(boost::bind(cameraCallback,_1, _2, _3));
	ros::spin();
    
	return 0;
}
